#include "detector/detect_node.hpp"

namespace detector
{

UsbCamNode::UsbCamNode() : it_(nh_)
{
    // 从参数服务器获取参数
    ros::NodeHandle private_nh("~");
    ros::Rate rate(1);
    private_nh.param("camera_index", camera_index_, 2);
    private_nh.param("frame_id", frame_id_, std::string("usb_cam"));


    // cap_.set(cv::CAP_PROP_AUTO_EXPOSURE, 0.25);  // 手动曝光
    // cap_.set(cv::CAP_PROP_EXPOSURE, 1);         // 曝光值30
    
    cap_.open(camera_index_);

        // 初始化摄像头
    cap_.set(cv::CAP_PROP_FRAME_WIDTH, 640);   // 宽度640像素
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, 480);  // 高度480像素
    
    while (ros::ok()&&!cap_.isOpened()) {
        try {
            cap_.open(camera_index_);
        } catch (const cv::Exception& e) {
            ROS_WARN_THROTTLE(1.0, "Camera error: %s", e.what());
        }
        ros::Duration(0.5).sleep();
        ROS_ERROR("Could not open video stream"); 

    }

    // 设置发布者
    image_pub_ = it_.advertise("image_raw", 1);

    ROS_INFO("USB Camera node started");
}




void UsbCamNode::publishImages()
{
    cv::Mat frame;
    sensor_msgs::ImagePtr msg;

    ros::Rate loop_rate(30); // 30Hz

    while (ros::ok()) {
        cap_ >> frame;
        if(frame.empty()) {
            ROS_WARN("Empty frame received");
            cap_.open(camera_index_);
            ros::Duration(0.1).sleep();  // 精确休眠0.1秒（100毫秒）
            continue;
        }
        ROS_INFO("Valid frame size: %dx%d", frame.cols, frame.rows);        
        // 转换OpenCV图像为ROS消息
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
        msg->header.stamp = ros::Time::now();
        msg->header.frame_id = frame_id_;

        // 发布图像
        image_pub_.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
}

UsbCamNode::~UsbCamNode()
{
    if(cap_.isOpened()) {
        cap_.release();
    }
}

}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "usb_cam_node");
    
    try{
        detector::UsbCamNode usb_cam;
        usb_cam.publishImages();
    }catch (const std::exception& e) {
        ROS_ERROR("%s", e.what());
    }

    return 0;
}


